import airsim
import time

def print_drone_heights(client, drones):
    """打印所有无人机的当前高度"""
    for drone in drones:
        state = client.getMultirotorState(vehicle_name=drone)
        z = state.kinematics_estimated.position.z_val
        print(f"{drone} 当前高度: {-z:.2f} 米")  # AirSim中z轴是负数，取反后显示

def main():
    # 连接到AirSim模拟器
    client = airsim.MultirotorClient()
    client.confirmConnection()
    
    # 无人机名称列表
    drones = ['Drone1', 'Drone2', 'Drone3', 'Drone4', 'Drone5', 'Drone6']
    

    # 读取轨迹文件
    trajectory_file = "PositionsLog3.txt"  # 替换为你的文件路径
    trajectory_data = []

    with open(trajectory_file, 'r') as file:
        for line_number, line in enumerate(file):
            # 每隔100行选择一个节点
            if line_number % 100 != 0:
                continue
            
            # 去除行首尾空白字符
            line = line.strip()
            
            # 跳过空行
            if not line:
                continue
            
            # 解析每一行的数据（假设使用逗号分隔）
            try:
                # 将每行数据按逗号分隔，并转换为浮点数
                data = [float(x) if x else 0.0 for x in line.split(',')]  # 空值替换为0.0
                trajectory_data.append(data)
            except ValueError as e:
                print(f"解析行时出错: {line}")
                print(f"错误信息: {e}")
                continue



    # 各无人机的初始目标高度
    initial_altitudes = {
        'Drone1': -20,  # z轴方向AirSim里是负数，因为是向上的
        'Drone2': -30,
        'Drone3': -30,
        'Drone4': -20,
        'Drone5': -25,
        'Drone6': -25
    }
    
    # 启用API控制并起飞各无人机
    for drone in drones:
        client.enableApiControl(True, drone)
        client.armDisarm(True, drone)
        client.takeoffAsync(vehicle_name=drone).join()
    
    # 打印起飞后的高度
    print("起飞后高度：")
    print_drone_heights(client, drones)
    
    # Drone2 和 Drone3 先升高10米，同时y方向减小5米
    move_to_position_futures = []
    for drone in ['Drone2', 'Drone3']:
        # 获取当前无人机的位置
        state = client.getMultirotorState(vehicle_name=drone)
        current_position = state.kinematics_estimated.position
        
        # 计算目标位置
        target_z = initial_altitudes[drone] + 20  # 升高10米
        target_y = current_position.y_val - 3  # y方向减小5米
        
        # 使用 moveToPositionAsync 控制无人机移动到目标位置
        move_to_position_futures.append(client.moveToPositionAsync(
            current_position.x_val,  # x方向保持不变
            target_y,                # y方向减小5米
            target_z,                # z方向升高10米
            5,                       # 速度
            vehicle_name=drone
        ))
        
    # 等待Drone2和Drone3到达目标位置
    for future in move_to_position_futures:
        future.join()
    
    # 打印Drone2和Drone3升高后的高度
    print("Drone2 和 Drone3 升高10米后高度：")
    print_drone_heights(client, drones)
    
    # 等待5秒
    time.sleep(5)
    
    # 所有无人机一起升高到各自的目标高度
    move_to_height_futures = []
    for drone in drones:
        target_z = initial_altitudes[drone]
        move_to_height_futures.append(client.moveToZAsync(target_z, 5, vehicle_name=drone))
    
    # Drone5 和 Drone6 升高到25米
    for drone in ['Drone5', 'Drone6']:
        target_z = -25  # 升高到25米
        move_to_height_futures.append(client.moveToZAsync(target_z, 5, vehicle_name=drone))
    
    # 等待所有无人机到达目标高度
    for future in move_to_height_futures:
        future.join()
    
    # 打印所有无人机升高后的高度
    print("所有无人机升高到目标高度后：")
    print_drone_heights(client, drones)
    
    # 等待10秒
    time.sleep(10)

      ##################################    沿着txt中的点同步移动       ###########################################################################
    # 同步移动到每个节点
    for node in trajectory_data:
        time_stamp = node[0]  # 时间戳（如果需要可以用于同步）
        print(f"正在移动到时间戳 {time_stamp} 的节点")
        
        # 所有无人机同时移动到当前节点
        move_futures = []
        for i, drone in enumerate(drones):
            # 计算当前无人机的坐标索引
            x_index = 1 + 3 * i  # x坐标索引
            z_index = 2 + 3 * i  # z坐标索引
            y_index = 3 + 3 * i  # y坐标索引
            
            # 获取目标位置
            target_x = node[x_index]
            target_z = node[z_index]-25   # 远离地面
            target_y = node[y_index]
            
            print(f"{drone} 正在移动到目标点: ({target_x}, {target_y}, {target_z})")
            
            # 使用 moveToPositionAsync 控制无人机移动到目标位置
            move_futures.append(client.moveToPositionAsync(
                target_x,  # x 方向目标
                target_y,  # y 方向目标
                target_z,  # z 方向目标
                3,         # 速度（单位：米/秒）
                vehicle_name=drone
            ))
        
        # 等待所有无人机到达当前节点
        for future in move_futures:
            future.join()
        
        print(f"所有无人机已到达时间戳 {time_stamp} 的节点\n")

    print("所有无人机已完成所有节点移动")
      ##################################     降落       ###########################################################################
    
    # Drone1、Drone2、Drone3、Drone4 同时开始降落
    land_futures = []
    for drone in ['Drone1', 'Drone2', 'Drone3', 'Drone4']:
        if drone in ['Drone1', 'Drone4']:
            # Drone1 和 Drone4 直接降落到地面
            land_futures.append(client.moveToZAsync(0, 3, vehicle_name=drone))
        else:
            # Drone2 和 Drone3 降落到离地面 10 米的高度
            land_futures.append(client.moveToZAsync(-10, 3, vehicle_name=drone))

    # 等待所有无人机到达目标高度
    for future in land_futures:
        future.join()

    # 打印Drone1和Drone4降落后的高度
    print("Drone1 和 Drone4 降落后高度：")
    print_drone_heights(client, ['Drone1', 'Drone4'])

    # 打印Drone2和Drone3降落到 10 米高度后的高度
    print("Drone2 和 Drone3 降落到 10 米高度后：")
    print_drone_heights(client, ['Drone2', 'Drone3'])

    # 等待5秒
    time.sleep(5)

    # Drone2 和 Drone3 同时朝 y 方向增加 4 米
    move_to_position_futures = []
    for drone in ['Drone2', 'Drone3']:
        # 获取当前无人机的位置
        state = client.getMultirotorState(vehicle_name=drone)
        current_position = state.kinematics_estimated.position
        
        # 计算目标位置
        target_y = current_position.y_val + 6  # y方向增加 4 米
        target_z = current_position.z_val  # z方向保持不变
        
        # 使用 moveToPositionAsync 控制无人机移动到目标位置
        move_to_position_futures.append(client.moveToPositionAsync(
            current_position.x_val,  # x方向保持不变
            target_y,                # y方向增加 4 米
            target_z,                # z方向保持不变
            5,                       # 速度
            vehicle_name=drone
        ))

    # 等待Drone2和Drone3到达目标位置
    for future in move_to_position_futures:
        future.join()

    # 打印Drone2和Drone3移动后的高度和位置
    print("Drone2 和 Drone3 朝 y 方向增加 4 米后：")
    for drone in ['Drone2', 'Drone3']:
        state = client.getMultirotorState(vehicle_name=drone)
        position = state.kinematics_estimated.position
        print(f"{drone} 当前高度: {-position.z_val:.2f} 米, y 位置: {position.y_val:.2f} 米")

    # Drone2 和 Drone3 同时降落到地面
    move_to_z_futures = []
    for drone in ['Drone2', 'Drone3']:
        # 使用 moveToZAsync 控制无人机降落到地面
        move_to_z_futures.append(client.moveToZAsync(0, 5, vehicle_name=drone))

    # 等待Drone2和Drone3降落到地面
    for future in move_to_z_futures:
        future.join()

    # 打印Drone2和Drone3降落后的高度和位置
    print("Drone2 和 Drone3 降落后：")
    for drone in ['Drone2', 'Drone3']:
        state = client.getMultirotorState(vehicle_name=drone)
        position = state.kinematics_estimated.position
        print(f"{drone} 当前高度: {-position.z_val:.2f} 米, y 位置: {position.y_val:.2f} 米")

    # Drone5 和 Drone6 自由降落到地面
    for drone in ['Drone5', 'Drone6']:
        client.moveToZAsync(0, 3, vehicle_name=drone).join()

    # 打印Drone5和Drone6降落后的高度
    print("Drone5 和 Drone6 降落后高度：")
    print_drone_heights(client, ['Drone5', 'Drone6'])

    # 所有无人机降落完成后，解除控制
    for drone in drones:
        client.landAsync(vehicle_name=drone).join()
        client.armDisarm(False, drone)
        client.enableApiControl(False, drone)
    
    # 打印降落后的高度
    print("降落后高度：")
    print_drone_heights(client, drones)
    
if __name__ == "__main__":
    main()